/*****************************************************************************
*  OpenST Basic tool library                                                 *
*  Copyright (C) 2021 Xinkai.Chen  chen_xinkai@126.com.                      *
*                                                                            *
*  This file is part of Weeding Robot Project.                               *
*                                                                            *
*  This program is free software; you can redistribute it and/or modify      *
*  it under the terms of the GNU General Public License version 3 as         *
*  published by the Free Software Foundation.                                *
*                                                                            *
*  You should have received a copy of the GNU General Public License         *
*  along with OST. If not, see <http://www.gnu.org/licenses/>.               *
*                                                                            *
*  Unless required by applicable law or agreed to in writing, software       *
*  distributed under the License is distributed on an "AS IS" BASIS,         *
*  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.  *
*  See the License for the specific language governing permissions and       *
*  limitations under the License.                                            *
*                                                                            *
*  @file     zed_actuator_checker_calibration_node.cpp                       *
*  @brief    棋盘格标定相机与执行机构之间的坐标转换矩阵                             *
*  Details.                                                                  *
*                                                                            *
*  @author   Xinkai.Chen                                                     *
*  @email    chen_xinkai@126.com                                             *
*  @version  1.0.0.1(版本号)                                                  *
*  @date     2021.09.22                                                      *
*  @license  GNU General Public License (GPL)                                *
*                                                                            *
*----------------------------------------------------------------------------*
*  Remark         : Description                                              *
*----------------------------------------------------------------------------*
*  Change History :                                                          *
*  <Date>     | <Version> | <Author>       | <Description>                   *
*----------------------------------------------------------------------------*
*  2021/09/22 | 1.0.0.1   | Xinkai.Chen    | Create file                     *
*----------------------------------------------------------------------------*
*                                                                            *
*****************************************************************************/
#include <ros/ros.h>
#include <iostream>
#include <boost/bind.hpp>
#include <vector>
#include <string>
#include <string>

#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/synchronizer.h>

#include <ros/spinner.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/CameraInfo.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/CameraInfo.h>

#include <opencv4/opencv2/opencv.hpp>
#include <opencv4/opencv2/core.hpp>
#include <opencv4/opencv2/calib3d.hpp>
#include <opencv4/opencv2/imgproc.hpp>

#include "zed_actuator_calibration.h"
#include "stereo_camera_ranging.h"
#include "zed_get_coordinate.h"

#define ZED_DEPTH_TOPIC "/zed2/zed_node/depth/depth_registered"
#define ZED_LEFT_IMAGE_TOPIC "/zed2/zed_node/left/image_rect_color"

#define CHESSBORAD_IMAGE_PATH "/home/chen/1.png"

ros::Publisher result_image_pub;

/**
 * @brief 如果中心点深度值不正确，从中心点开始逐次尝试用8邻域中的平均深度值填充中心点
 * @param u 图像中中心点横坐标
 * @param v 图像中中心点纵坐标
 * @param image_height 图像高度
 * @param image_width 图像宽度
 * @param image 深度图像数组头指针
 * @param try_times 尝试的次数，默认为10
 *
 * @return 返回计算得到的深度值，-1表示计算失败
 *     -<em>-1</em> fail
 *     -<em>true</em> succeed
 */
float depth_calculate(int u, int v, int image_height, int image_width,const float* image, int try_times = 10)
{
  static int directions[4][2] = {{0,1},{1,0},{0,-1},{-1,0}};
  assert(u <= image_width && v <= image_height);

  float depth, current_depth;
  int times;
  int current_u,current_v, current_edgelen;
  int valid_num;
  int nextRow, nextColumn;

  times = 1;
  depth = 0;
  while (times <= try_times) {
    std::vector<std::vector<bool>> visited(image_height, std::vector<bool>(image_width));
    valid_num = 0;
    int directionIndex = 0;
    current_edgelen = 2*times + 1;
    current_u = u - times;
    current_v = v - times;
    int row = 0,column=0;
    /* 图像矩阵8邻域遍历 */
    for(int i=0;i<8*times;i++)
    {
      current_u += row;
      current_v += column;
      current_depth = image[current_u + current_v * image_width];
      if(!(std::isnan(current_depth) || std::isinf(current_depth)))
      {
        depth += current_depth;
        ++valid_num;
      }
      nextRow = row + directions[directionIndex][0];
      nextColumn = column + directions[directionIndex][1];
      if(nextRow <0 || nextRow >= current_edgelen || nextColumn < 0 || nextColumn >= current_edgelen || visited[nextRow][nextColumn])
      {
        directionIndex = (directionIndex + 1) % 4;
      }
      row += directions[directionIndex][0];
      column += directions[directionIndex][1];
    }
    if(!std::isnan(depth) && !std::isinf(depth))
    {
      break;
    }
    times++;
  }
  if(valid_num == 0)
  {
    std::cout << " false ";
    return -1;
  }
  else {
    std::cout << "*********************************" << std::endl;
    return depth / valid_num;
  }
}

/**
 * @brief 提取棋盘格图像中的棋盘格角点并在图像中绘制角点位置
 * @param input_image 传入传出参数 待提取特征的棋盘格图像
 * @param image_corners_vec 传入传出参数 存放角点图像坐标的容器
 *
 * @return void 无返回值
 *
 */
void extract_chessborad_corners(cv::Mat& input_image, std::vector<cv::Point2f>& image_corners_vec)
{
  std::string text;
  bool flag;
  int thickness = 2;
  int font_face = cv::FONT_HERSHEY_COMPLEX;
  double font_scale = 0.5;
  int index;

  /* 从棋盘格图像中提取角点图像坐标 */
  flag = cv::findChessboardCorners(input_image, cv::Size(6,9), image_corners_vec);
  if(flag == 0)
  {
    return;
  }

  /* 角点图像坐标亚像素精度优化 */
  flag = cv::find4QuadCornerSubpix(input_image, image_corners_vec, cv::Size(5,5));
  if(flag == 0)
  {
    return;
  }
  /* 角点图像坐标绘制 */
  cv::drawChessboardCorners(input_image, cv::Size(6,9), image_corners_vec, true);

  for(auto i:image_corners_vec)
  {
    text = std::to_string(index);
    cv::putText(input_image, text, i, font_face, font_scale, cv::Scalar(0, 255, 255), thickness, 8, 0);
    index++;
  }

}

void callBck(const sensor_msgs::Image::ConstPtr& depth_image, const sensor_msgs::Image::ConstPtr& left_image_msg)
{
  cv_bridge::CvImagePtr left_cv_ptr;
  cv::Mat left_image, result_image;
  float* depths;
  std::vector<cv::Point2f> image_corners_vec;
  std::vector<cv::Point3f> image_corners_coordinate_point_vec;
  sensor_msgs::ImagePtr result_image_msg;
  std::string text, point_text;
  int index = 0;
  int sizes, width, height;
  int u,v,depth_image_index;
  cv::Point3f corners_coordinate_point;
  cv::Point2f corners_image_point;
  int thickness = 2;
  int font_face = cv::FONT_HERSHEY_COMPLEX;
  double font_scale = 0.4;

  depths = (float*)(&depth_image->data[0]);
  sizes = depth_image->data.size();
  width = depth_image->width;
  height = depth_image->height;
  left_cv_ptr = cv_bridge::toCvCopy(left_image_msg, sensor_msgs::image_encodings::BGR8);
  left_image = left_cv_ptr->image;
  left_image.copyTo(result_image);
  cv::cvtColor(left_image,left_image,cv::COLOR_RGB2GRAY);
  extract_chessborad_corners(left_image, image_corners_vec);

  if(image_corners_vec.size() == 0)
  {
    return;
  }

  for(auto i:image_corners_vec)
  {
    u = i.x;
    v = i.y;
    depth_image_index = u + v * depth_image->width;
    if(depth_image_index < 0)
    {
      depth_image_index = 0;
    }
    else if (depth_image_index > sizes / 4)
    {
      depth_image_index = sizes / 4;
    }
    corners_coordinate_point.z = depths[depth_image_index];
    if(std::isnan(corners_coordinate_point.z) || std::isinf(corners_coordinate_point.z))
    {
      corners_coordinate_point.z = depth_calculate(u, v, height, width, depths);
    }
    corners_coordinate_point.x = (corners_coordinate_point.z * (u - 632.2350)) / 2014.6162;
    corners_coordinate_point.y = (corners_coordinate_point.z * (v - 323.1177)) / 2025.0555;
    image_corners_coordinate_point_vec.push_back(corners_coordinate_point);

    point_text = "x:" + std::to_string(corners_coordinate_point.x) + " y:" + std::to_string(corners_coordinate_point.y)
        + " z:" + std::to_string(corners_coordinate_point.z);
    std::cout << point_text << std::endl;
  }

  for(auto j:image_corners_coordinate_point_vec)
  {
    corners_image_point = image_corners_vec.at(index);
    //point_text = std::to_string(index) + "x:" + std::to_string(i.x) + " y:" + std::to_string(i.y) + " z:" + std::to_string(i.z);
    //std::cout << point_text << std::endl;
    text = std::to_string(index);
    cv::putText(result_image, text, corners_image_point, font_face, font_scale, cv::Scalar(0, 255, 0), thickness, 8, 0);
    index++;
  }
  result_image_msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", result_image).toImageMsg();
  result_image_pub.publish(result_image_msg);
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "zed_actuator_checker_calibration_node");
  if(!ros::ok())
  {
    return 0;
  }
  ros::NodeHandle nh;

  result_image_pub = nh.advertise<sensor_msgs::Image>("/zed_actuator_calibration_node/result_image", 1);
  message_filters::Subscriber<sensor_msgs::Image> zed_depth(nh, ZED_DEPTH_TOPIC, 1);
  message_filters::Subscriber<sensor_msgs::Image> zed_left_img(nh, ZED_LEFT_IMAGE_TOPIC, 1);
  typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image> synPolicy;
  message_filters::Synchronizer<synPolicy> sync(synPolicy(10), zed_depth, zed_left_img);
  sync.registerCallback(boost::bind(&callBck, _1, _2));

  ros::AsyncSpinner spinner(3);

  ros::spin();
  return 0;

  //  cv::Mat input_image;
  //  std::vector<cv::Point2f> chessborad_image_corners_vec;

  //  input_image = cv::imread(CHESSBORAD_IMAGE_PATH, 0);
  //  extract_chessborad_corners(input_image, chessborad_image_corners_vec);
  //  std::cout << "Detect " << chessborad_image_corners_vec.size() << " corners" << std::endl;
  //  return 0;
}
